#include <iostream>

#include "BlRobotCtrl.hpp"

using std::placeholders::_1;
namespace rvt = rviz_visual_tools;

BlRobotCtrl::BlRobotCtrl() : Node("BlRobotCtrl")
{
  
  subscription_ = this->create_subscription<yyrobot_msg::msg::ArmGoalState>(
      "BlRobotCtrl", 10, std::bind(&BlRobotCtrl::topic_callback, this, _1));
  jointStatePublisher_ = this->create_publisher<sensor_msgs::msg::JointState>("/borunte_arm/joint_states", 10);
  jointStatePublisher_2 = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
  
  std::chrono::milliseconds wallTimer(100000);
  //timer_ptr_ = this->create_wall_timer( wallTimer , std::bind(&BlRobotCtrl::doControl, this) );
  
  init();
  
  std::cout << "construction called" << std::endl;
}

BlRobotCtrl::~BlRobotCtrl(){}

void BlRobotCtrl::init(){
	const std::shared_ptr<Node> robot_node = std::shared_ptr<Node>(this);
  robot_model_loaderPtr = std::shared_ptr<robot_model_loader::RobotModelLoader>( new robot_model_loader::RobotModelLoader(robot_node, "robot_description"));
  robot_model_ptr = robot_model_loaderPtr->getModel();

robot_statePtr = moveit::core::RobotStatePtr(new moveit::core::RobotState(robot_model_ptr));
  joint_model_group = robot_statePtr->getJointModelGroup(PLANNING_GROUP);

  planning_scene = planning_scene::PlanningScenePtr(new planning_scene::PlanningScene(robot_model_ptr));
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

  joint_names = joint_model_group->getJointModelNames();

  std::vector<double> joint_values;
  RCLCPP_INFO(this->get_logger(), "Print joint values, start ...");
  // planning_scene->getCurrentState().printStatePositions();
  planning_scene->getCurrentState().copyJointGroupPositions(joint_model_group, joint_values);

  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(this->get_logger(), "Joint %s: %d / %f", joint_names[i].c_str(), (int)(joint_values[i] * 180 / 3.1416),
                joint_values[i]);
  }
  RCLCPP_INFO(this->get_logger(), "Print joint values, end.");
  
  if (!this->get_parameter("planning_plugin", planner_plugin_name)){
  	RCLCPP_FATAL(this->get_logger(), "Could not find planner plugin name");
  	}
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager"));
  }
  catch (pluginlib::PluginlibException& ex)
  {
    RCLCPP_FATAL(this->get_logger(), "Exception while creating planning plugin loader %s", ex.what());
  }

  rclcpp::Node::SharedPtr thisNodePtrTmp = this->shared_from_this();
  const rclcpp::Node::SharedPtr& thisNodePtr = thisNodePtrTmp;
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model_ptr, thisNodePtr, this->get_namespace()))
      RCLCPP_FATAL(this->get_logger(), "Could not initialize planner instance");
    RCLCPP_INFO(this->get_logger(), "Using planning interface '%s'", planner_instance->getDescription().c_str());
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (const auto& cls : classes)
      ss << cls << " ";
    RCLCPP_ERROR(this->get_logger(), "Exception while loading planner '%s': %s\nAvailable plugins: %s",
                 planner_plugin_name.c_str(), ex.what(), ss.str().c_str());
  }
  display_publisherPtr = this->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path", 1);

  move_groupPtr = new moveit::planning_interface::MoveGroupInterface(thisNodePtr, PLANNING_GROUP);
  // move_groupPtr =  std::shared_ptr<moveit_visual_tools::MoveItVisualTools>(moveGroupPtrTmp);//
  // std::shared_moveGroupPtr->shared_from_this();
  // namespace rvt = rviz_visual_tools;
  visual_toolsPtr.reset(new moveit_visual_tools::MoveItVisualTools(thisNodePtr, "base_link", "the_arm", move_groupPtr->getRobotModel()));
  visual_toolsPtr->enableBatchPublishing( true );
  visual_toolsPtr->deleteAllMarkers();
  
   Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_toolsPtr->publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);
  
  visual_toolsPtr->trigger();

  // Remote control is an introspection tool that allows users to step through a high level script
  //   via buttons and keyboard shortcuts in RViz 
  visual_toolsPtr->loadRemoteControl();
  
  /*
  //test
  geometry_msgs::msg::PoseStamped pose;
  pose.header.frame_id = "base_link";
  pose.pose.position.x = 0.3;
  pose.pose.position.y = 0.4;
  pose.pose.position.z = 0.75;
  pose.pose.orientation.w = 1.0;
  plan(&pose);
  */
}

void BlRobotCtrl::doControl()
{
  static int counter = 0;
  counter++;
  std::cout << "doControl called,counter="<<counter << std::endl;


  //RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_toolsPtr->publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations 
  visual_toolsPtr->trigger();
  
}
//////////////////////////////////////////////////////////////////////////////////////

void BlRobotCtrl::topic_callback(const yyrobot_msg::msg::ArmGoalState& msg)
{
	static int counter = 0;
  counter++;
  std::cout << "=========topic_callback called,counter="<<counter << std::endl;
  
  RCLCPP_INFO(this->get_logger(), "cmd: %s , id: %s, goal_type: %s", msg.cmd.c_str(), msg.id.c_str(),
              msg.goal_type.c_str());
  // RCLCPP_INFO(this->get_logger())

  geometry_msgs::msg::PoseStamped pose;
  if (msg.goal_type == "W")
  {
    pose.header.frame_id = "base_link";
    pose.pose.position.x = msg.position[0];
    pose.pose.position.y = msg.position[1];
    pose.pose.position.z = msg.position[2];
    pose.pose.orientation.x = msg.position[3];
    pose.pose.orientation.y = msg.position[4];
    pose.pose.orientation.z = msg.position[5];
    pose.pose.orientation.w = msg.position[6];
  }
  else {}
  
  plan(&pose );
}
void BlRobotCtrl::plan(geometry_msgs::msg::PoseStamped* posePtr){
  geometry_msgs::msg::PoseStamped& pose = *posePtr;
  planning_interface::MotionPlanRequest req;
  planning_interface::MotionPlanResponse res;

  std::vector<double> tolerance_pose(3, 0.01);
  std::vector<double> tolerance_angle(3, 0.01);

  moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("link6", pose, tolerance_pose, tolerance_angle);

  req.group_name = PLANNING_GROUP;
  req.goal_constraints.push_back(pose_goal);

  planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    RCLCPP_ERROR(this->get_logger(), "Could not compute plan successfully");
    return;
  }

  std::cout << "========= 22 ========" << std::endl;
  // Visualize the result
  // ^^^^^^^^^^^^^^^^^^^^

  // this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
  // moveit_msgs::msg::DisplayTrajectory display_trajectory;

  // Visualize the trajectory
  moveit_msgs::msg::MotionPlanResponse response;
  res.getMessage(response);

  std::vector<double> joint_valuesYY;
  res.trajectory_->getLastWayPoint().copyJointGroupPositions(joint_model_group, joint_valuesYY);
std::cout << "========= 33 ========" << std::endl;
if( NULL==this->display_trajectoryPtr.get() ){
	
	std::cout << "========= 33.001 ========" << std::endl;
	display_trajectoryPtr.reset( new moveit_msgs::msg::DisplayTrajectory() );
}
	display_trajectoryPtr->trajectory.clear();
  this->display_trajectoryPtr->trajectory_start = response.trajectory_start;
  std::cout << "========= 33 .1========" << std::endl;
  display_trajectoryPtr->trajectory.push_back(response.trajectory);
  std::cout << "========= 33 .2========" << std::endl;
  
   //visual_toolsPtr->enableBatchPublishing( true );
 
  visual_toolsPtr->publishTrajectoryLine(display_trajectoryPtr->trajectory.back(), joint_model_group);
  std::cout << "========= 33 .3========" << std::endl;
  visual_toolsPtr->trigger();
  display_publisherPtr->publish( *(display_trajectoryPtr.get() ) );
std::cout << "========= 44 ========" << std::endl;
  // Set the state in the planning scene to the final state of the last plan 
  robot_statePtr->setJointGroupPositions(joint_model_group,
                                         response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_statePtr.get());
std::cout << "========= 55 ========" << std::endl;
  // Display the goal state
  visual_toolsPtr->publishAxisLabeled(pose.pose, "goal_1");
  static Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  visual_toolsPtr->publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
  visual_toolsPtr->trigger();
std::cout << "========= 66 ========" << std::endl;
  RCLCPP_INFO(this->get_logger(), "Print current joint values, [1]");
  // planning_scene->getCurrentState().printStatePositions();
  std::vector<double> joint_values;
  robot_statePtr->copyJointGroupPositions(joint_model_group, joint_values);

  auto message2 = std_msgs::msg::String();
  // char[] msgData =
  message2.data = "data:[";
std::cout << "========= 77 ========" << std::endl;
  sensor_msgs::msg::JointState endJointState;
  int nJointCount = joint_names.size();
  rclcpp::Clock theClock;
  endJointState.header.stamp = theClock.now();
  endJointState.name.resize(nJointCount);
  endJointState.position.resize(nJointCount);  //=
std::cout << "========= 88 ========" << std::endl;
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(this->get_logger(), "Joint %s: %d / %f", joint_names[i].c_str(),
                (int)(1000 * joint_values[i] * 180 / 3.1416), joint_values[i]);
    if (i > 0)
    {
      message2.data += ",";
    }
    message2.data += joint_names[i] + ":" + std::to_string((int)(1000 * joint_values[i] * 180 / 3.1416));

    endJointState.name[i] = joint_names[i];
    endJointState.position[i] = joint_values[i];
  }
  message2.data += "]";

  // publisher_ -> publish(message2);
std::cout << "========= 99 ========" << std::endl;
  jointStatePublisher_->publish(endJointState);
  jointStatePublisher_2->publish(endJointState);

  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(this->get_logger(), "Joint %s: %d", joint_names[i].c_str(), (int)(joint_valuesYY[i] * 180 / 3.1416));
  }

  RCLCPP_INFO(this->get_logger(), "Print Current joint values, [1]");

  // char firstChar = msg.data.c_str()[0];
}


int main(int argc, char* argv[])
{
std::cout << "main called" << std::endl;
rclcpp::Logger LOGGERxx = rclcpp::get_logger("BorunteArm");
RCLCPP_INFO( LOGGERxx, "before construct BlRobotCtl");

  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BlRobotCtrl>());
  rclcpp::shutdown();
  return 0;
}
